#include "attitude_estimate_task.h"

#define WHEEL_RADIUS 0.1

float attitude_theta = 0;
float attitude_speed = 0;

void attitude_value_estimate(float* acc_vehicle, float delta_t)
{
    float acc_world[3] = {};
    float average_motor_speed = 0;
    arm_atan2_f32(attitude.matrix_data[3], attitude.matrix_data[6], &attitude_theta);//TODO
    average_motor_speed = (motor_left.data.speed - motor_right.data.speed) / 2; //TODO CHECK
    arm_mat_vec_mult_f32(&attitude.matrix, acc_vehicle, acc_world);
    float fresh_speed_acc = acc_world[1] * delta_t + attitude_speed;
    float fresh_speed_motor = average_motor_speed * WHEEL_RADIUS;
    attitude_speed = fresh_speed_acc * 0.8 + fresh_speed_motor * 0.2;
}

void StartAttitudeEstimateTask(void *argument)
{
	bmi088_init(&bmi088_data, &hspi1, GPIOA, GPIO_PIN_4, GPIOB, GPIO_PIN_0);
	bmi088_range_set(&bmi088_data, BMI088_ACC_RANGE_24, BMI088_GYRO_RANGE_2000);
    bmi088_gyro_offset_set(&bmi088_data, - 0.000327947899, - 0.00378698087, -0.000335433666);
	bmi088_temp_set(&bmi088_data, 45);
    taskENTER_CRITICAL();
    float acc_init[3] = {bmi088_data.acc.x, bmi088_data.acc.y, bmi088_data.acc.z};
    attitude_init(acc_init, 1000);
    attitude_value_estimate(acc_init, 0.001);
    taskEXIT_CRITICAL();

    extern EventGroupHandle_t initReadyEventHandle;
    EventBits_t event_mask = 0b01;
    xEventGroupSetBits(initReadyEventHandle, event_mask);

    vTaskDelay(1);

    while (1)
    {
        taskENTER_CRITICAL();
        float gyro[3] = {bmi088_data.gyro_rate.x, bmi088_data.gyro_rate.y, bmi088_data.gyro_rate.z};
		float acc[3] = {bmi088_data.acc.x, bmi088_data.acc.y, bmi088_data.acc.z};
		attitude_execute(gyro, acc);

        attitude_value_estimate(acc, 0.001);
        taskEXIT_CRITICAL();
        vTaskDelay(1);
    }
    
}